#include "vehicle_lateral_speed3dis_mpc.h"
namespace auto_ros
{
namespace control
{

VehicleLateralSpeed3disMpc::VehicleLateralSpeed3disMpc(const std::string dyn_mpc_kf_config_path_name, const std::string vehicle_lmpc_yaml_name, double rate)
	: VehicleLMpcBase(dyn_mpc_kf_config_path_name, vehicle_lmpc_yaml_name, rate)
{
	init_mpc();
} //
void VehicleLateralSpeed3disMpc::init_mpc()
{
	dis_dyn_ptr_ = vehicle_dyn_ptr_->discretization(1 / rate_, "tustin");
	/*	Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr,
		unsigned int mpc_windows,
		unsigned int max_xu_constraint,//1
		unsigned int max_delta_x,//0
		unsigned int max_delta_u,//1
		unsigned int max_final_constraint,//0
		unsigned int max_xu_ae =0,//0
		unsigned int max_xu_slack_ineq = 0, //2
		unsigned int max_x_final_ae = 0,
		unsigned int max_x_final_slack_ineq = 0, //0
		unsigned int max_delta_u_ae = 1,
		unsigned int max_delta_u_slack_ineq = 0, // 0
		unsigned int max_delta_x_ae = 0,
		unsigned int max_delta_x_slack_ineq = 0);*/

	vehicle_mpc_ptr_ = std::make_shared<Mpc>(dis_dyn_ptr_, mpc_yaml_["mpc_windows"].as<int>(),
											 1 /*max_xu_constraint*/, 0, 1 /*max_delta_u*/, 0, 0,
											 2 /*max_xu_slack_ineq = 0*/, 0, 0, 1 /*max_delta_u_ae*/, 0, 0, 0);
	vehicle_mpc_ptr_->set_xu_affine({}, {}, {0}, {1.0}, 0, -mpc_yaml_["max_tire_f_angle"].as<double>() / 180 * 3.14,
									mpc_yaml_["max_tire_f_angle"].as<double>() / 180 * 3.14);
	vehicle_mpc_ptr_->set_xu_slack_ineq({0, 1}, {1 / 1.0, vehicle_dyn_ptr_->dyn_yaml_["lf"].as<double>() / 1.0}, {0}, {-1}, 0,
										-mpc_yaml_["alpha_f_max"].as<double>() / 180 * 3.14,
										mpc_yaml_["alpha_f_max"].as<double>() / 180 * 3.14);
	vehicle_mpc_ptr_->set_xu_slack_ineq({0, 1}, {1 / 1.0, -vehicle_dyn_ptr_->dyn_yaml_["lr"].as<double>() / 1.0}, {}, {}, 1,
										-mpc_yaml_["alpha_r_max"].as<double>() / 180 * 3.14,
										mpc_yaml_["alpha_r_max"].as<double>() / 180 * 3.14);
	vehicle_mpc_ptr_->set_delta_u_ae(0, 0, 0);
	vehicle_mpc_ptr_->set_delta_u_ae_w({mpc_yaml_["delta_tire_steering_f_w"].as<double>() * rate_ * rate_});

	vehicle_mpc_ptr_->set_xu_slack_ineq_w({mpc_yaml_["tire_f_alpha_w"].as<double>(), mpc_yaml_["tire_r_alpha_w"].as<double>()});

	//int set_delta_u(unsigned int constraint_index, unsigned int u_index, double min, double max);
	vehicle_mpc_ptr_->set_delta_u(0, 0, -mpc_yaml_["max_steering_rate"].as<double>() / rate_,
								  mpc_yaml_["max_steering_rate"].as<double>() / rate_);
}
bool VehicleLateralSpeed3disMpc::step(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
									  const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
									  double speed, const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, double tire_f_alpha_w,
									  double tire_r_alpha_w)
{
	vehicle_dyn_ptr_->speed_update_matrix(speed);
	vehicle_dyn_ptr_->discrete(dis_dyn_ptr_, 1 / rate_, "tustin");
	/*bool Mpc::update_mpc(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
					 const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
					 const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, const std::vector<double> &xu_ae_w,
					 const std::vector<double> &xu_slack_ineq_w, const std::vector<double> &x_final_ae_w,
					 const std::vector<double> &x_final_slack_ineq_w, const std::vector<double> &delta_u_ae_w,
					 const std::vector<double> &delta_u_slack_ineq_w, const std::vector<double> &delta_x_ae_w,
					 const std::vector<double> &delta_x_slack_ineq_w)*/
	if (!vehicle_mpc_ptr_->update_mpc(x0, u_fb, x_ref, u_ref, distur_ref, q, r, {}, {tire_f_alpha_w, tire_r_alpha_w},
									  {}, {}, {}, {}, {}, {}))
	{
		std::cout << "\033[31m Error:VehicleLateralSpeed3disMpc::step failed,vehicle_mpc_ptr_->update_mpc. \033[0m" << std::endl;
		return false;
	}
	if (!(vehicle_mpc_ptr_->mpc_solve()))
	{
		std::cout << "\033[31m Error:VehicleLateralSpeed3disMpc::step failed,vehicle_mpc_ptr_->mpc_solve(). \033[0m" << std::endl;
		return false;
	}
	return true;
}
bool VehicleLateralSpeed3disMpc::step(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
									  double kappa, double speed)
{
	Eigen::VectorXd y(kf_lateral_ptr_->con_ptr_->out_dim());
	y << x0(0), x0(1);
	Eigen::VectorXd control_last(kf_lateral_ptr_->con_ptr_->control_dim());
	control_last << u_fb(0);
	kf_lateral_ptr_->step(control_last, y, speed);
	Eigen::VectorXd state_esti = kf_lateral_ptr_->state_esti();
	Eigen::MatrixXd q_temp = Eigen::MatrixXd::Zero(vehicle_dyn_ptr_->state_dim(), vehicle_dyn_ptr_->state_dim());
	q_temp(0, 0) = mpc_yaml_["lateral_speed_factor"].as<double>();
	q_temp(1, 1) = mpc_yaml_["yaw_rate_factor"].as<double>();
	q_temp(2, 2) = mpc_yaml_["lateral_error_factor"].as<double>();
	q_temp(3, 3) = mpc_yaml_["speed_gama_factor"].as<double>();
	Eigen::MatrixXd r_temp = Eigen::MatrixXd::Zero(vehicle_dyn_ptr_->control_dim(), vehicle_dyn_ptr_->control_dim());
	if (std::abs(x0[2]) <= 0.01 && std::abs(x0[3]) < 0.01) //std::abs(distur_ref(0, 0)) < 0.001
	{
		r_temp(0, 0) = mpc_yaml_["steering_f_factor_max"].as<double>();
	}
	else
	{
		r_temp(0, 0) = mpc_yaml_["steering_f_factor_min"].as<double>();
	}

	Eigen::VectorXd x_ref(dis_dyn_ptr_->state_dim());
	Eigen::VectorXd u_ref(dis_dyn_ptr_->control_dim());
	Eigen::VectorXd e_y_ref(1);
	e_y_ref << 0.0;
	double speed_threshold = vehicle_dyn_ptr_->dyn_yaml_["speed_threshold"].as<double>();
	if (std::abs(speed) < speed_threshold)
	{
		if (speed >= 0)
			speed = speed_threshold;
		else
			speed = -speed_threshold;
	}
	vehicle_dyn_ptr_->speed_update_matrix(speed);
	vehicle_dyn_ptr_->discrete(dis_dyn_ptr_, 1 / rate_, "tustin");
	/*calc_steady_state_ctr(
	const Eigen::VectorXd &distur_ref, const Eigen::VectorXd &y_controlled_ref,
	Eigen::VectorXd &steady_state, Eigen::VectorXd &steady_control)*/
	Eigen::VectorXd distur_ref(3);
	distur_ref << kappa, 0, 0;
	//vehicle_dyn_ptr_->calc_steady_state_ctr(distur_ref, e_y_ref, x_ref, u_ref);
	dis_dyn_ptr_->calc_steady_state_ctr(distur_ref, e_y_ref, x_ref, u_ref);
	x_ref_ = x_ref;
	u_ref_ = u_ref;
	vehicle_mpc_ptr_->set_xu_slack_ineq({0, 1}, {1 / speed, vehicle_dyn_ptr_->dyn_yaml_["lf"].as<double>() / speed}, {0}, {-1}, 0,
										-mpc_yaml_["alpha_f_max"].as<double>() / 180 * 3.14,
										mpc_yaml_["alpha_f_max"].as<double>() / 180 * 3.14);
	vehicle_mpc_ptr_->set_xu_slack_ineq({0, 1}, {1 / speed, -vehicle_dyn_ptr_->dyn_yaml_["lr"].as<double>() / speed}, {}, {}, 1,
										-mpc_yaml_["alpha_r_max"].as<double>() / 180 * 3.14,
										mpc_yaml_["alpha_r_max"].as<double>() / 180 * 3.14);

	double tire_f_alpha_w = mpc_yaml_["tire_f_alpha_w"].as<double>();
	double tire_r_alpha_w = mpc_yaml_["tire_r_alpha_w"].as<double>();

	/*bool Mpc::update_mpc(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
					 const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
					 const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, const std::vector<double> &xu_ae_w,
					 const std::vector<double> &xu_slack_ineq_w, const std::vector<double> &x_final_ae_w,
					 const std::vector<double> &x_final_slack_ineq_w, const std::vector<double> &delta_u_ae_w,
					 const std::vector<double> &delta_u_slack_ineq_w, const std::vector<double> &delta_x_ae_w,
					 const std::vector<double> &delta_x_slack_ineq_w)*/

	if (!vehicle_mpc_ptr_->update_mpc(x0, u_fb, x_ref, u_ref, distur_ref, q_temp, r_temp, {}, {tire_f_alpha_w, tire_r_alpha_w},
									  {}, {}, {}, {}, {}, {}))
	{
		std::cout << "\033[31m Error:VehicleLateralSpeed3disMpc::step failed,vehicle_mpc_ptr_->update_mpc. \033[0m" << std::endl;
		return false;
	}
	if (!(vehicle_mpc_ptr_->mpc_solve()))
	{
		std::cout << "\033[31m Error:VehicleLateralSpeed3disMpc::step failed,vehicle_mpc_ptr_->mpc_solve(). \033[0m" << std::endl;
		return false;
	}
	return true;
}

bool VehicleLateralSpeed3disMpc::reset_kf()
{
	Eigen::VectorXd state_esti(4);
	state_esti << 0, 0, 0, 0;
	Eigen::MatrixXd state_cov_esti(4, 4);
	state_cov_esti << 0.01, 0, 0, 0,
		0, 0.01, 0, 0,
		0, 0, 0.01, 0,
		0, 0, 0, 0.01;

	return kf_lateral_ptr_->reset(state_esti, state_cov_esti);
}
bool VehicleLateralSpeed3disMpc::reset_kf(const Eigen::VectorXd &state_esti)
{
	Eigen::MatrixXd state_cov_esti(4, 4);
	state_cov_esti << 0.01, 0, 0, 0,
		0, 0.01, 0, 0,
		0, 0, 0.01, 0,
		0, 0, 0, 0.01;
	return kf_lateral_ptr_->reset(state_esti, state_cov_esti);
}
} // namespace control
} // namespace auto_ros
